#include "fuzzy.h"


float target;//系统的控制目标
float actual;//采样获得的实际值
float e;     //误差
float e_pre; //上一次的误差
float de;    //误差的变化率
float emax;  //误差基本论域上限
float demax; //误差辩化率基本论域的上限
float umax;  //输出的上限
float Ke;    //Ke=n/emax,量化论域为[-3,-2,-1,0,1,2,3]
float Kde;   //Ke=n/demax,量化论域为[-3,-2,-1,0,1,2,3]
float Ku;    //Ke=umax/n,量化论域为[-3,-2,-1,0,1,2,3]

int ruleMatrix[7][7]={  {NB,NB,NM,NM,NS,ZO,ZO},
                        {NB,NB,NM,NS,NS,ZO,PS},
                        {NM,NM,NM,NS,ZO,PS,PS},
                        {NM,NM,NS,ZO,PS,PM,PM},
                        {NS,NS,ZO,PS,PS,PM,PM},
                        {NS,ZO,PS,PM,PM,PM,PB},
                        {ZO,ZO,PM,PM,PM,PB,PB}};

int mf_paras[21]= {-3,-3,-2,-3,-2,-1,-2,-1,0,-1,0,1,0,1,2,1,2,3,2,3,3};

void Fuzzy_contrller_Init(float e_max,float de_max,float u_max)
{
    emax = e_max;
    demax = de_max;
    u_max = u_max;
    target = 0;
    actual = 0;
    e = target - actual;
    e_pre = 0;
    de = e - e_pre;
    Ke = (N/2)/emax;
    Kde = (N/2)/demax;
    Ku = umax/(N/2);
}


//三角隶属度函数
float trimf(float x,float a,float b,float c)
{
   float u;
   if(x>=a&&x<=b)
	   u=(x-a)/(b-a);
   else if(x>b&&x<=c)
	   u=(c-x)/(c-b);
   else
	   u=0.0;
   return u;
}

//实现模糊控制
float realize(float t,float a)
{
	float u_e[N],u_de[N],u_u[N];
	int u_e_index[3],u_de_index[3];//假设一个输入最多激活3个模糊子集
	float u;
	int M;
	target = t;
	actual = a;
    e = target - actual;
	de = e - e_pre;
	e = Ke * e;
	de = Kde * de;
    M = 3;  //三角函数有三个参数
	int j=0;
	for(int i=0;i<N;i++)
	{
		u_e[i]=trimf(e,mf_paras[i*M],mf_paras[i*M+1],mf_paras[i*M+2]);//e模糊化，计算它的隶属度
		if(u_e[i]!=0)
            u_e_index[j++]=i;                                              //存储被激活的模糊子集的下标，可以减小计算量
	}
	for(;j<3;j++)u_e_index[j]=0;
	M=3;              //三角函数有三个参数
	j=0;
	for(int i=0;i<N;i++)
	{
		u_de[i]=trimf(de,mf_paras[i*M],mf_paras[i*M+1],mf_paras[i*M+2]);//de模糊化，计算它的隶属度
		if(u_de[i]!=0)
			u_de_index[j++]=i;                                                    //存储被激活的模糊子集的下标，可以减小计算量
	}
	for(;j<3;j++)u_de_index[j]=0;

	float den=0,num=0;
	for(int m=0;m<3;m++)
		for(int n=0;n<3;n++)
		{
		   num+=u_e[u_e_index[m]]*u_de[u_de_index[n]]*ruleMatrix[u_e_index[m]][u_de_index[n]];
		   den+=u_e[u_e_index[m]]*u_de[u_de_index[n]];
		}
	u = num/den;
	u = Ku * u;
	if(u>=umax)   u=umax;
	else if(u<=-umax)  u=-umax;
	e_pre=e;
	return u;
}
